#include "includes.h"


char temp[5];
int i;
uint8_t canint = 0;


ISR(INT4_vect)
{
    canint = 1;
}


uint8_t getcan()
{
	uint8_t temp = canint;
	canint = 0;
	return temp;
}


void can_interrupt()
{
	if (getcan() == 1)
	{
		can_message_t msg;							// can_message shell
		char temp[10];
	    uint8_t rx_status = mcp_rx_status();
	    uint8_t buffer = rx_status>>6;
	
		//	Receive the can message
	    can_message_receive(buffer, &msg);
		// Check if the message is from our group
		if(msg.data[0]==GROUP)
		{
			switch (msg.id)
			{
				case JOY_COORDS:
					// Use Y-coordinates to set the servo position
					calculate_servo((int8_t) msg.data[1]);
					// Use X-coordinates to set the motor speed and direction
					calculate_motor((int8_t) msg.data[2]);
					break;

				case SHOOT:
					if(i==1)
					{
						USART_print("FIRE!\r\n");
						shoot(1);
					}
					else
					{
						shoot(0);
					}
					break;
				case WII:
					switch (msg.data[1])
					{
						case MOTOR:
							i=0;
							temp[0] = msg.data[2];
							temp[1] = msg.data[3];
							temp[2] = msg.data[4];
							temp[3] = msg.data[5];
							temp[4] = '\0';
							i = atoi(temp);
							calculate_motor(i);
							_delay_ms(1);
							break;
						case SERVO:
							i=0;
							temp[0] = msg.data[2];
							temp[1] = msg.data[3];
							temp[2] = msg.data[4];
							temp[3] = msg.data[5];
							temp[4] = '\0';
							i = atoi(temp);
							calculate_servo(i);
							_delay_ms(1);
							break;
						case SHOOT:
							i=0;
							temp[0] = msg.data[2];
							temp[1] = '\0';
							i = atoi(temp);
							if(i==1)
							{
								USART_print("FIRE!\r\n");
								shoot(1);
							}
							else
							{
								shoot(0);
							}
							break;
						default:
							break;
					
					}
					break;

				default:
			
					break;
			}
		}
		_delay_us(1);

		// Reset the can interrupt flag
	    mcp_bit_modify(CAN_INTF,0,(1<<CAN_RX0IF));
	}
}


void can_init()
{
	mcp_init();
}

void can_message_send(uint8_t buffer, can_message_t* msg)
{
	uint8_t highId	= (uint8_t) (msg->id>>3);
	uint8_t lowId	= (uint8_t) (msg->id<<5);

	switch (buffer)
	{
		case BUFFER0:
			mcp_write_byte(TXB0SIDH, highId);	
			mcp_write_byte(TXB0SIDL, lowId);
			mcp_write_byte(TXB0DLC, msg->length);
			for(i=0;i<msg->length;i++)
			{
 				mcp_write_byte(TXB0D0+i, msg->data[i]); 
			}
			mcp_rts(BUFFER0);
			break;
		
		case BUFFER1:
			mcp_write_byte(TXB1SIDH, highId);	
			mcp_write_byte(TXB1SIDL, lowId);
			mcp_write_byte(TXB1DLC, msg->length);
			for(i=0;i<msg->length;i++)
			{
 				mcp_write_byte(TXB1D0+i, msg->data[i]); 
			}
			mcp_rts(BUFFER1);
			break;

		default:
			USART_print("Unknown buffer in can_message_send!\r\n");
			break;
	}
}

void can_message_receive(uint8_t buffer, can_message_t* received)
{
	uint8_t highId;
	uint8_t lowId;

	switch (buffer)
	{
		case BUFFER0:
			highId = mcp_read_byte(RXB0SIDH);
			highId = highId << 3;
			lowId = mcp_read_byte(RXB0SIDL);
			lowId = lowId >> 5;
			received->id = highId | lowId;

			received->length = mcp_read_byte(RXB0DLC);

			for (i=0;i<received->length;i++)
			{
				received->data[i] = mcp_read_byte(RXB0D0+i);
			}
			mcp_bit_modify(0x2c, 0x00, 0x01);
			break;

		case BUFFER1:
			highId = mcp_read_byte(RXB1SIDH);
			highId = highId << 3;
			lowId = mcp_read_byte(RXB1SIDL);
			lowId = lowId >> 5;
			received->id = highId | lowId;

			received->length = mcp_read_byte(RXB1DLC);

			for (i=0;i<received->length;i++)
			{
				received->data[i] = mcp_read_byte(RXB1D0+i);
			}	
			mcp_bit_modify(0x2c, 0x00, 0x02);
			break;

		default:
			USART_print("Unknown buffer in can_message_receive!\r\n");
			break;
	}
}
